#include "Camera.h"
#include <cassert>
#include <cmath>
#include <iostream>
#include "smvec4f.h"
 
const double MMIN=1e-10;

CCamera::CCamera(void)
{
	mEye=smVec3f(1.0f,0.0f,0.0f);
	mUp=smVec3f(0.0f,0.0f,1.0f);
	mCenter=smVec3f(0.0f,0.0f,0.0f);
	mNear=1.0f;
	mRatio=1.333f;
	mFovy=90;
	update();
}

CCamera::~CCamera(void)
{
}

void CCamera::Set( const smVec3f& eye, const smVec3f& center,const smVec3f& up,  float near, float ratio, float fovy )
{
	mEye=eye;
	mUp=up;
	mCenter=center;
	mNear=near;
	mRatio=ratio;
	mFovy=fovy;

	smVec3f dirt=(center-eye);
	dirt.Normalize();
	float t=up.Dot(dirt);
	//	smVec3f::Dot(up,dirt);
	smVec3f normalup;
	if (t>MMIN){
		float b=dirt.Dot(dirt)/t;
		normalup=b*up-dirt;
	}
	else if (t<-(MMIN))
	{
		float b=(-1.0f*dirt.Dot(dirt))/t;
		normalup=(b*up+dirt);
	}
	else{
		normalup=up;
	}
	float debug=normalup.Dot(dirt);
	assert(fabs(debug)<MMIN);

	mUp=normalup;

	update();

		
}


const smVec3f& CCamera::GetEyePoint() const
{
	return mEye;
}

void CCamera::GenerateModelViewMatrix()
{
	smMatrix44f model (1,0,0,0,
					0,1,0,0,
					0,0,1,0,
					-mEye.x(),-mEye.y(),-mEye.z(),1);
	smVec3f Y=mUp;
	Y.Normalize();
	smVec3f Z=(mEye-mCenter);
	Z.Normalize();
	smVec3f X=Y.Cross(Z);
	X.Normalize();
	smMatrix44f view(X.x(),Y.x(),Z.x(),0,
				  X.y(),Y.y(),Z.y(),0,
				  X.z(),Y.z(),Z.z(),0,
				  0,0,0,1);
	mModelView=model*view;
}

smVec3f CCamera::PointModelViewTransform( const smVec3f& v3 ) const
{
	smVec4f v4(v3.x(),v3.y(),v3.z(),1);
	smVec4f ret=v4*mModelView;
	return smVec3f(ret[0],ret[1],ret[2]); 
}

void CCamera::Forward( float step )
{
	smVec3f dirt=(mCenter-mEye);
	dirt.Normalize();
	mEye=mEye+step*dirt;
	mCenter=mCenter+step*dirt;
	update();
}

void CCamera::Backward( float step )
{
	smVec3f dirt=(mCenter-mEye);
	dirt.Normalize();
	mEye=mEye-step*dirt;
	mCenter=mCenter-step*dirt;
	update();
}

void CCamera::update()
{
	//std::cout<<mEye.x()<<"	"<<mEye.y()<<"	"<<mEye.z()<<"	"<<std::endl;
	GenerateModelViewMatrix();
	mOriginalPreCompute=PointModelViewTransform(smVec3f(0.0f,0.0f,0.0f));
}

smVec3f CCamera::VectorModelViewTransform( const smVec3f& v3 ) const
{
	return PointModelViewTransform(v3)-mOriginalPreCompute;
	
}

void CCamera::Left( float step )
{
	smVec3f dirt=(mCenter-mEye);
	//	float length=dirt.Length();
	dirt=mUp.Cross(dirt);
	dirt.Normalize();
	//mCenter=mEye+length*dirt;
	mEye+=step*dirt;
	mCenter+=step*dirt;
	update();
}

void CCamera::Right( float step )
{
	smVec3f dirt=(mCenter-mEye);
	//	float length=dirt.Length();
	dirt=dirt.Cross(mUp);
	dirt.Normalize();
	//mCenter=mEye+length*dirt;
	mEye+=step*dirt;
	mCenter+=step*dirt;
	update();
}

void CCamera::Up( float step )
{
	mCenter+=step*mUp;
	mEye+=step*mUp;
	update();
}

void CCamera::Down( float step )
{
	mCenter-=step*mUp;
	mEye-=step*mUp;
	update();
}